#include "Vector4.h"
#include "Vector2.h"

using namespace Nezha;


const Vector2 Vector2::ZERO(0.0f,0.0f);
const Vector2 Vector2::UNIT_X(1.0f,0.0f);
const Vector2 Vector2::UNIT_Y(0.0f,1.0f);

const Vector3 Vector3::ZERO(0.0f,0.0f,0.0f);
const Vector3 Vector3::UNIT_X(1.0f,0.0f,0.0f);
const Vector3 Vector3::UNIT_Y(0.0f,1.0f,0.0f);
const Vector3 Vector3::UNIT_Z(0.0f,0.0f,1.0f);
const Vector3 Vector3::UNIT_ALL(1.0f,1.0f,1.0f);

const Vector4 Vector4::ZERO(0.0f, 0.0f, 0.0f, 0.0f);


void Vector3::FastNormalizes(Vector3 *p, u32 numVectors)
{
	float len = 0;
	for(u32 i = 0;i < numVectors;i++)
	{
		len = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
		len = FastEval::FastInvSqrt(len);
		p[i].x *= len;
		p[i].y *= len;
		p[i].z *= len;
	}	
}

void Vector3::orthonormalize(Vector3 &U, Vector3 &V, Vector3 &W)
{
	//   u0 = v0/|v0|
	//   u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0|
	//   u2 = (v2-(u0*v2)u0-(u1*v2)u1)/|v2-(u0*v2)u0-(u1*v2)u1|

	U.normalize();

	float fDot0 = U.dot(V);
	V -= fDot0*U;
	V.normalize();

	fDot0 = U.dot(W);
	float fDot1 = V.dot(W);
	W -= fDot0*U + fDot1*V;
	W.normalize();
}